#!/usr/bin/env python

# Copyright (c) 2013-2018, Rethink Robotics Inc.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

"""
SDK Joint Position Example: keyboard
"""
import argparse

import rospy
import math,time
import intera_interface
from intera_motion_interface import (
    MotionTrajectory,
    MotionWaypoint,
    MotionWaypointOptions
)
from intera_examples import JointRecorder
from PyQt5 import QtWidgets
from PyQt5.QtCore import QThread,pyqtSignal,QTime
from PyQt5.QtWidgets import QGraphicsPixmapItem,QGraphicsScene
from PyQt5.QtGui import QImage,QPixmap
from sawyerUI import Ui_Form
from intera_motion_msgs.msg import TrajectoryOptions
from intera_interface import CHECK_VERSION
from geometry_msgs.msg import PoseStamped

class set_xyz(QThread):
    set_xyz_signal = pyqtSignal(str)

    def __init__(self,limb):
        super(set_xyz,self).__init__()
        self.working = False
        self.limb = limb
        self.traj_options = TrajectoryOptions()
        self.traj_options.interpolation_type = TrajectoryOptions.CARTESIAN


    def run(self):
        while self.working:
            wpt_opts = MotionWaypointOptions(max_linear_speed=0.6,
                                             max_linear_accel=0.6,
                                             max_rotational_speed=1.57,
                                             max_rotational_accel=1.57,
                                             max_joint_speed_ratio=1.0)
            waypoint = MotionWaypoint(options=wpt_opts.to_msg(), limb=limb)
            traj = MotionTrajectory(trajectory_options=self.traj_options, limb=self.limb)
            endpoint_state = self.limb.tip_state('right_hand')
            pose = endpoint_state.pose
            pose.position.x = self.x
            pose.position.y = self.y
            pose.position.z = self.z
            pose.orientation.x = self.ox
            pose.orientation.y = self.oy
            pose.orientation.z = self.oz
            pose.orientation.w = self.ow
            poseStamped = PoseStamped()
            poseStamped.pose = pose
            joint_angles = self.limb.joint_ordered_angles()
            waypoint.set_cartesian_pose(poseStamped, 'right_hand', joint_angles)
            traj.append_waypoint(waypoint.to_msg())
            result = traj.send_trajectory()
            self.set_xyz_signal.emit('reach!')
            self.close_thread()


    def set_x_y_z(self,x,y,z,ox,oy,oz,ow):
        self.working = True
        self.x = x
        self.y = y
        self.z = z
        self.ox = ox
        self.oy = oy
        self.oz = oz
        self.ow = ow
        self.set_xyz_signal.emit('start set x y z ......')
        self.start()

    def close_thread(self):
        self.working = False
        self.wait()



class read_joint_angles(QThread):
    get_joint_angles = pyqtSignal(list)

    def __init__(self,limb):
        super(read_joint_angles,self).__init__()
        self.limb = limb

    def run(self):
        while True:
            current_joint = self.limb.joint_ordered_angles()
            self.get_joint_angles.emit([int(joint/math.pi*180) for joint in current_joint])
            time.sleep(1)
            # self.sleep(1)

class record(QThread,Ui_Form):
    record_signal = pyqtSignal(str)
    def __init__(self):
        super(record,self).__init__()
        self.record_label = False

    def run(self):
        while self.record_label:
            print("Recording. Press Ctrl-C to stop.")
            self.recorder.record()

    def start_record(self,filename,record_rate):
        self.record_label = True
        self.filename = filename
        self.record_rate = record_rate
        self.recorder = JointRecorder(self.filename, self.record_rate)
        self.start()
        self.record_signal.emit('start record '+ filename.split('/')[-1] + ' ......')

    def stop_record(self):
        self.recorder.stop()
        self.record_signal.emit('stop record!')
        self.record_label = False
        self.wait()

class set_joint_angles(QThread):
    set_joint_signal = pyqtSignal(str)
    def __init__(self,limb):
        super(set_joint_angles,self).__init__()
        self.set_joint_label = False
        self.working = True
        self.limb = limb
        self.wpt_opts = MotionWaypointOptions(max_joint_speed_ratio=0.5,
                                              max_joint_accel=0.5)

    def run(self):
        while self.working:
            traj = MotionTrajectory(limb=self.limb)
            waypoint = MotionWaypoint(options=self.wpt_opts.to_msg(), limb=self.limb)
            current_joints = self.limb.joint_ordered_angles()
            waypoint.set_joint_angles(joint_angles=current_joints)
            traj.append_waypoint(waypoint.to_msg())
            if len(current_joints) != 7:
                rospy.logerr('The number of joint_angles must be %d', len(current_joints))
                return None
            waypoint.set_joint_angles(joint_angles=self.target_joints)
            traj.append_waypoint(waypoint.to_msg())
            result = traj.send_trajectory(timeout=None)
            self.set_joint_signal.emit('reach!')
            self.close_thread()

    def set_joint(self,target_joints):
        self.set_joint_signal.emit('start set joint!')
        self.working = True
        self.target_joints = target_joints
        self.start()

    def close_thread(self):
        self.working = False
        self.wait()

class button_control_joint(QThread):
    def __init__(self,limb):
        super(button_control_joint,self).__init__()
        self.limb = limb
        self.joints = self.limb.joint_names()
        self.press_label = False
        self.working = False

    def run(self):
        while self.press_label:
            current_position = self.limb.joint_angle(self.joints[self.joint_num])
            self.limb.set_joint_positions({self.joints[self.joint_num]:current_position + self.delta_joint})

    def control_joint(self,joint_num,delta_joint):
        self.press_label = True
        self.joint_num = joint_num
        self.delta_joint = delta_joint
        self.start()

    def stop_control_joint(self):
        self.press_label = False
        self.wait()

class go_home(QThread):
    go_home_signal = pyqtSignal(str)
    def __init__(self,limb):
        super(go_home,self).__init__()
        self.limb = limb
        self.working = False

    def run(self):
        while self.working:
            self.limb.move_to_neutral()
            self.go_home_signal.emit('reach!')
            self.close_thread()

    def start_go_home(self):
        self.go_home_signal.emit('start go home!')
        self.working = True
        self.start()

    def close_thread(self):
        self.working = False
        self.wait()

class MyWindow(QtWidgets.QWidget,Ui_Form):
    set_joint_signal = pyqtSignal(list)

    def __init__(self,limb):
        super(MyWindow,self).__init__()
        self.setupUi(self)
        self.limb = intera_interface.Limb(limb)

        # ===== thread =====
        self.thread_read_joint_angles = read_joint_angles(self.limb)
        self.thread_read_joint_angles.get_joint_angles.connect(self.display_joint_angles)
        self.thread_read_joint_angles.start()

        self.thread_set_joint_angles = set_joint_angles(self.limb)
        self.thread_set_joint_angles.set_joint_signal.connect(self.print_text)
        self.set_joint_signal.connect(self.thread_set_joint_angles.set_joint)

        self.thread_go_home = go_home(self.limb)
        self.go_home_pushButton.clicked.connect(self.thread_go_home.start_go_home)
        self.thread_go_home.go_home_signal.connect(self.print_text)

        self.thread_set_xyz = set_xyz(self.limb)
        self.thread_set_xyz.set_xyz_signal.connect(self.print_text)
        self.set_xyz_button.clicked.connect(lambda:self.thread_set_xyz.set_x_y_z(float(self.x_lineEdit.text()),
                                                                         float(self.y_lineEdit.text()),
                                                                         float(self.z_lineEdit.text()),
                                                                         float(self.ox_lineEdit.text()),
                                                                         float(self.oy_lineEdit.text()),
                                                                         float(self.oz_lineEdit.text()),
                                                                         float(self.ow_lineEdit.text())))


        self.thread_record = record()
        self.thread_record.record_signal.connect(self.print_text)
        self.start_record_button.clicked.connect(
            lambda: self.thread_record.start_record(self.file_space_lineEdit.text(),
                                                    float(self.save_rate_lineEdit.text())))
        self.stop_record_button.clicked.connect(self.thread_record.stop_record)

        self.thread_button_control_joint = button_control_joint(self.limb)
        self.joint0_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(0, -0.1))
        self.joint0_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(0, 0.1))
        self.joint1_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(1, -0.1))
        self.joint1_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(1, 0.1))
        self.joint2_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(2, -0.1))
        self.joint2_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(2, 0.1))
        self.joint3_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(3, -0.1))
        self.joint3_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(3, 0.1))
        self.joint4_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(4, -0.1))
        self.joint4_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(4, 0.1))
        self.joint5_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(5, -0.1))
        self.joint5_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(5, 0.1))
        self.joint6_de.pressed.connect(lambda: self.thread_button_control_joint.control_joint(6, -0.1))
        self.joint6_in.pressed.connect(lambda: self.thread_button_control_joint.control_joint(6, 0.1))

        self.joint0_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint0_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint1_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint1_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint2_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint2_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint3_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint3_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint4_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint4_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint5_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint5_in.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint6_de.released.connect(self.thread_button_control_joint.stop_control_joint)
        self.joint6_in.released.connect(self.thread_button_control_joint.stop_control_joint)

        # ===== link signal and sort =====
        self.joint0_slider.valueChanged.connect(
            lambda: self.joint0_text.setText(str(self.joint0_slider.value())))
        self.joint1_slider.valueChanged.connect(
            lambda: self.joint1_text.setText(str(self.joint1_slider.value())))
        self.joint2_slider.valueChanged.connect(
            lambda: self.joint2_text.setText(str(self.joint2_slider.value())))
        self.joint3_slider.valueChanged.connect(
            lambda: self.joint3_text.setText(str(self.joint3_slider.value())))
        self.joint4_slider.valueChanged.connect(
            lambda: self.joint4_text.setText(str(self.joint4_slider.value())))
        self.joint5_slider.valueChanged.connect(
            lambda: self.joint5_text.setText(str(self.joint5_slider.value())))
        self.joint6_slider.valueChanged.connect(
            lambda: self.joint6_text.setText(str(self.joint6_slider.value())))

    def print_text(self,str):
        self.textEdit.append(str)

    def display_joint_angles(self,joint_angles):
        self.joint0_number.display(joint_angles[0])
        self.joint1_number.display(joint_angles[1])
        self.joint2_number.display(joint_angles[2])
        self.joint3_number.display(joint_angles[3])
        self.joint4_number.display(joint_angles[4])
        self.joint5_number.display(joint_angles[5])
        self.joint6_number.display(joint_angles[6])

    def stop(self):
        self.textEdit.append('Quit')
        # rospy.signal_shutdown('Quit')
        # self.thread_set_joint_angles.close_thread()
        # self.thread_go_home.close_thread()

    def set_joint(self):
        joint0_angle = self.threshold(int(self.joint0_text.text()))
        joint1_angle = self.threshold(int(self.joint1_text.text()))
        joint2_angle = self.threshold(int(self.joint2_text.text()))
        joint3_angle = self.threshold(int(self.joint3_text.text()))
        joint4_angle = self.threshold(int(self.joint4_text.text()))
        joint5_angle = self.threshold(int(self.joint5_text.text()))
        joint6_angle = self.threshold(int(self.joint6_text.text()))

        self.joint0_slider.setValue(joint0_angle)
        self.joint1_slider.setValue(joint1_angle)
        self.joint2_slider.setValue(joint2_angle)
        self.joint3_slider.setValue(joint3_angle)
        self.joint4_slider.setValue(joint4_angle)
        self.joint5_slider.setValue(joint5_angle)
        self.joint6_slider.setValue(joint6_angle)

        traget_joints = []
        traget_joints.append(joint0_angle / 180.0 * math.pi)
        traget_joints.append(joint1_angle / 180.0 * math.pi)
        traget_joints.append(joint2_angle / 180.0 * math.pi)
        traget_joints.append(joint3_angle / 180.0 * math.pi)
        traget_joints.append(joint4_angle / 180.0 * math.pi)
        traget_joints.append(joint5_angle / 180.0 * math.pi)
        traget_joints.append(joint6_angle / 180.0 * math.pi)
        self.set_joint_signal.emit(traget_joints)


    # ===== my func =====
    def threshold(self,num):
        if num>175:
            num = 175
        elif num<-175:
            num = -175
        return num

def main():
    """RSDK Joint Position Example: Keyboard Control

    Use your dev machine's keyboard to control joint positions.

    Each key corresponds to increasing or decreasing the angle
    of a joint on Sawyer's arm. The increasing and descreasing
    are represented by number key and letter key next to the number.
    """
    epilog = """
See help inside the example with the '?' key for key bindings.
    """
    rp = intera_interface.RobotParams()
    valid_limbs = rp.get_limb_names()
    if not valid_limbs:
        rp.log_message(("Cannot detect any limb parameters on this robot. "
                        "Exiting."), "ERROR")
        return
    arg_fmt = argparse.RawDescriptionHelpFormatter
    parser = argparse.ArgumentParser(formatter_class=arg_fmt,
                                     description=main.__doc__,
                                     epilog=epilog)
    parser.add_argument(
        "-l", "--limb", dest="limb", default=valid_limbs[0],
        choices=valid_limbs,
        help="Limb on which to run the joint position keyboard example"
    )
    args = parser.parse_args(rospy.myargv()[1:])

    print("Initializing node... ")
    rospy.init_node("sawyer_ui",disable_signals=False)
    print("Getting robot state... ")
    rs = intera_interface.RobotEnable(CHECK_VERSION)
    init_state = rs.state().enabled

    def clean_shutdown():
        print("\nExiting example.")

    rospy.on_shutdown(clean_shutdown)

    rospy.loginfo("Enabling robot...")
    rs.enable()
    return args.limb


if __name__ == "__main__":
    import sys
    limb = main()
    app = QtWidgets.QApplication(sys.argv)
    myshow = MyWindow(limb)
    myshow.show()




    sys.exit(app.exec_())
